/*************************************************************************
*
*	simulate_speed.c
*
*  Copyright 2007,2008 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module provides functions for simulating boat status.
*
*
**************************************************************************
*
*  version 1.0.8, 10/17/08, SBS
*   - broken out from simulate_boat
*
*  version 1.0.7, 10/4/08, SBS
*   - documentation
*
*  version 1.0.6, 8/27/08, SBS
*   - use status_SetTimeString()
*   - add random offset to boat start positions
*
*  version 1.0.5, 5/30/08, SBS
*    - move conversions (lat, long, deg, rad, etc) into conversions.h
*    - add conditionals for simulating buoys
*    - cleanup
*
*  version 1.0.4, 5/29/08, SBS
*    - add UpdateBoatSpeed()
*    - add status "direction" = net motion direction
*    - use direction instead of heading in positionfromheadingspeed
*    - move InitBoat here from simulate_engineering
*
*  version 1.0.3, 5/18/08, SBS
*    - add boat rotating into wind during free drift
*
*  version 1.0.2, 5/17/08, SBS
*    - modify drift to include both wind and water 
*    - add turn rate to heading changes
*
*  version 1.0.1, 5/12/08, SBS
*    - add drift
*
*  version 1.0, 1/13/08, SBS
*    - split from simulate_engineering.c
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @file 
*  @ingroup plat_eng_simulate
*  @brief
*  This module provides functions for simulating boat status.
*  <hr>
**/

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>

#include "waypoints.h"
#include "platform_types.h"
#include "platform_modes.h"
#include "status.h"
#include "conversions.h"

//#include "../initialize.h" 

#include "science_main.h"
//#include "boatmodes.h"

#include "simulate_speed.h"


// contributions of water current and wind to drift
//#define DRIFT_FROM_WATER 0.4
//#define DRIFT_FROM_WIND  0.1
#define DRIFT_FROM_WATER 0.
#define DRIFT_FROM_WIND  0.


/********************************************************
*
* UpdateBoatSpeed()
*/
/**
* @brief
* Determines boat speed (groundspeed and waterspeed)
*   by combining propulsion, water current, and wind.
*
*
********************************************************/
void UpdateBoatSpeed(
			STATUS *s
			, int deltat)
{
	SCI_DBL sd1,sd2,sd3,sd4;
	POSITION p1;
	VELOCITY current,wind,prop,net;


	//**********
	// extract current boat position
	//**********		
	p1 = status_PositionFromStatus(*s);

	//**********
	// retrieve water current
	//**********
	sd1 = science_main(CURRENTSPEED,p1);
	sd2 = science_main(CURRENTANGLE,p1);

	//**********
	// convert to x,y representation
	//**********
	current.dLatMps = sd1.dValue * cos(RADIANS(sd2.dValue));
	current.dLonMps = sd1.dValue * sin(RADIANS(sd2.dValue));


/*
	if(s->id >= SIMBUOYOFFSET){ // don't bother with wind if it's a buoy

		s->dSpeed_mps = sqrt(current.dLatMps*current.dLatMps + current.dLonMps*current.dLonMps);
		s->dDirectionDeg = DEGREES(atan2(current.dLonMps,current.dLatMps));
		s->dWaterspeed_mps = 0.0;
		
	}else{
*/	
		//**********
		// retrieve wind vector
		//**********		
		sd3 = science_main(WINDSPEED,p1);
		sd4 = science_main(WINDANGLE,p1);
			
		//**********
		// convert to x,y representation
		//**********
		wind.dLatMps = sd3.dValue * cos(RADIANS(sd4.dValue));
		wind.dLonMps = sd3.dValue * sin(RADIANS(sd4.dValue));
	

		switch(s->nMode){
		
		// need to add momentum terms here
		
			case MODE_AWAITINSTR:		
			case MODE_PAUSE:
			case MODE_INTERRUPT:		
				//**********
				// no propulsion
				//**********
				prop.dLatMps = 0.0;
				prop.dLonMps = 0.0;
				
				break;
				
			case MODE_TRANSIT:		
				//**********
				// with propulsion	
				//**********
				prop.dLatMps = s->CurrWpt.dSpeedMps * cos(RADIANS(s->dHdgDeg));
				prop.dLonMps = s->CurrWpt.dSpeedMps * sin(RADIANS(s->dHdgDeg));
			
				break;
		
			default:
				break;
		}
	
		//**********
		// combine propulsion, wind and water to get groundspeed
		//**********
		net.dLatMps = prop.dLatMps + current.dLatMps * DRIFT_FROM_WATER + wind.dLatMps * DRIFT_FROM_WIND;
		net.dLonMps = prop.dLonMps + current.dLonMps * DRIFT_FROM_WATER + wind.dLonMps * DRIFT_FROM_WIND;
		
		s->dSpeed_mps = sqrt(net.dLatMps*net.dLatMps + net.dLonMps*net.dLonMps);
		s->dDirectionDeg = DEGREES(atan2(net.dLonMps,net.dLatMps));


		//**********
		// combine propulsion and wind to get waterspeed
		//**********
	
		net.dLatMps = prop.dLatMps + wind.dLatMps * DRIFT_FROM_WIND;
		net.dLonMps = prop.dLonMps + wind.dLonMps * DRIFT_FROM_WIND;
				
//		s->dWaterspeed_mps = sqrt(net.dLatMps*net.dLatMps + net.dLonMps*net.dLonMps);
		s->dWaterspeed_mps = s->dSpeed_mps;

/*
	}			
*/
	if(s->dDirectionDeg >= 360.){
		s->dDirectionDeg -= 360.;
	}else if(s->dDirectionDeg < 0.){
		s->dDirectionDeg += 360.;
	}

	return;
}




